#include "resetstate.h"
#include "fsm.h"
#include "masters/vehiclecontroller.h"
#include "fsm/pid.h"
#include "Eigen/Eigen"
#include "device/drdc.h"
#include "device/dhdc.h"
using namespace std;
using namespace Eigen;
using namespace vehicle;
ResetState::ResetState()
{

}

/**
 * @brief PositionState::Update
 * @param fsm
 *
 *
 */


void ResetState::Update(FSM *fsm)
{
    switch (fsm->vehicle_controller_->GetRunningStatus())
    {
    case main_controller::kRCPause:
        fsm->SetState(BaseState::kPauseState);
        break;
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
        if(fsm->vehicle_controller_->GetConnectionStatus()==main_controller::kConnection2RobotLost)
        {
            fsm->SetState(BaseState::kPauseState);
        }
        else
        {
            char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();

            ushort sn=0;
            int ret=dhdGetSerialNumber(&sn,master_device);
            if(ret!=0)
            {
                fsm->vehicle_controller_->SetMasterDeviceIndex(-1);
                master_device=-1;
            }

            if(master_device<0)
            {
                fsm->SetState(BaseState::kPauseState);
            }
            else
            {

                if(!drdIsMoving(master_device))
                {
                    cout<<"arrived "<<endl;
                    fsm->SetState(BaseState::kWorkingState);
                }
                else
                {
                    cout<<"is moving "<<endl;
                }
            }
        }
        break;
    default:
        break;
    }

}

/**
 * @brief PositionState::OnEntry
 * 若是从待机状态进入，发送命令将从臂移动至初始位置，根据当前从臂初始位置和映射比例计算主手目标位置。
 * 若是从暂停状态进入，从臂不动，根据当前从臂当前位置和映射比例计算主手目标位置。//delete
 * 若是从力控状态进入，从臂不动，根据当前从臂当前位置和映射比例计算主手目标位置。
 */

void ResetState::OnEntry(FSM *fsm)
{
    cout<<"entry position state"<<endl;
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
    if(master_device<0)
        return;
    dhdSetForceAndGripperForce(0,0,0,0,master_device);

    drdRegulatePos(true,master_device);
    drdRegulateGrip(false,master_device);
    drdMoveToPos(0,0,0,false,master_device);

}

void ResetState::OnExit(FSM *fsm)
{
    cout<<"exit position state"<<endl;
//    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
//    if(master_device<0)
//        return;
//    drdRegulatePos(false,master_device);

//    if(fsm->vehicle_controller_->SetMasterForce(Eigen::Vector3d::Zero()))
//        cout<<"failed to clear feedback force"<<endl;
}
